#include "least_square.h"
#include <fstream>

int main(int argc, char *argv[])
{
  std::ofstream f;
  f.open("data.txt");
  // obj = A * sin(Bx) + C * cos(D*x) - F
  // there are 4 parameter: A, B, C, D.
  int num_params = 4;

  // generate random data using these parameter
  int total_data = 100;

  Eigen::VectorXd input(total_data);
  Eigen::VectorXd output(total_data);

  double A = 5, B = 1, C = 10, D = 2;
  // load observation data
  std::vector<double> x_vec;
  x_vec.clear();
  for (int i = 0; i < total_data; i++)
  {
    // generate a random variable [-10 10]
    double x = 10.0 * ((random() % 1000) / 1000.0) - 5.0;
    x_vec.push_back(x);
    double noise  = (random() % 1000) / 1000.0;
    double y      = A * sin(B * x) + C * cos(D * x) + noise;
    double real_y = y - noise;

    f << x << "," << y << "," << real_y << std::endl;

    input(i)  = x;
    output(i) = y;
  }

  // gauss the parameters
  Eigen::VectorXd params_gn(num_params);
  // init gauss
  params_gn << 3.6, 1.4, 5.2, 1.7;

  Eigen::VectorXd params_lm = params_gn;
  Eigen::VectorXd params_dl = params_gn;

  LeastSquare ls;

  f << std::endl;
  ls.GaussNewton(input, output, params_gn);
  for (size_t i = 0; i < x_vec.size(); i++)
  {
    double x = x_vec[i];
    double y = params_gn(0) * sin(params_gn(1) * x) + params_gn(2) * cos(params_gn(3) * x);
    f << x << "," << y << std::endl;
  }

  f << std::endl;
  ls.LM(input, output, params_lm);
  for (size_t i = 0; i < x_vec.size(); i++)
  {
    double x = x_vec[i];
    double y = params_lm(0) * sin(params_lm(1) * x) + params_lm(2) * cos(params_lm(3) * x);
    f << x << "," << y << std::endl;
  }

  f << std::endl;
  ls.DogLeg(input, output, params_dl);
  for (size_t i = 0; i < x_vec.size(); i++)
  {
    double x = x_vec[i];
    double y = params_dl(0) * sin(params_dl(1) * x) + params_dl(2) * cos(params_dl(3) * x);
    f << x << "," << y << std::endl;
  }

  std::cout << "Gauss-Newton parameter: " << std::endl << params_gn << std::endl << std::endl << std::endl;
  std::cout << "Levenberg-Marquardt parameter: " << std::endl << params_lm << std::endl << std::endl << std::endl;
  std::cout << "Dog-Leg parameter: " << std::endl << params_dl << std::endl << std::endl << std::endl;

  f.close();
  return 0;
}
